#include "Maths/Vector3D.h"



namespace SimPhy{



Vector3D::Vector3D()
{
    for(unsigned int i=0; i<4; i++)
        m_components[i]=0.0;
}


Vector3D::Vector3D(real const x, real const y, real const z)
{
    m_components[0]=x;
    m_components[1]=y;
    m_components[2]=z;
    m_components[3]=0.0;
}

real Vector3D::operator[](unsigned int i) const
{
    if(i<2)
        return m_components[i];
    else
        return m_components[2];
}

real& Vector3D::operator[](unsigned int i)
{
    if(i<2)
        return m_components[i];
    else
        return m_components[2];
}


std::ostream& operator<<( std::ostream &stream, Vector3D const& vector )
{
    stream << "(" << vector[0] << "," << vector[1] << "," << vector[2] << ")";
    return stream;
}

std::istream &operator>>( std::istream &stream, Vector3D &vector)
{
    for(unsigned int i=0; i<3; i++)
        stream >> vector[i];

    return stream;
}

bool operator==(Vector3D const& v1, Vector3D const& v2)
{
   bool isEqual=true;
   unsigned int i=0;

   while(isEqual && i<3)
   {
       isEqual= (v1[i]==v2[i]);
       i++;
   }

   return isEqual;
}

bool operator!=(Vector3D const& v1, Vector3D const& v2)
{
    if(v1==v2)
        return false;
    else
        return true;
}

//Addition
Vector3D& Vector3D::operator+=(Vector3D const& vector)
{
    for(unsigned int i=0; i<3; i++)
        m_components[i]+=vector[i];

    return *this;
}
Vector3D operator+(Vector3D const& v1, Vector3D const& v2)
{
    Vector3D copy(v1);
    copy+=v2;
    return copy;
}

//Substraction
Vector3D& Vector3D::operator-=(Vector3D const& vector)
{
    for(unsigned int i=0; i<3; i++)
        m_components[i]-=vector[i];

    return *this;
}
Vector3D operator-(Vector3D const& v1, Vector3D const& v2)
{
    Vector3D copy(v1);
    copy-=v2;
    return copy;
}

//Scalar product
Vector3D& Vector3D::operator*=(real const scalar)
{
    for(unsigned int i=0; i<3; i++)
        m_components[i]*=scalar;

    return *this;
}

Vector3D operator*(Vector3D const& vector, real const scalar)
{
    Vector3D copy(vector);
    copy *= scalar;
    return copy;
}

Vector3D operator*(real const scalar, Vector3D const& vector)
{
    return vector*scalar;
}

Vector3D operator-(Vector3D const& vector)
{
    return -1*vector;
}

// Dot product
real operator*(Vector3D const& v1, Vector3D const& v2)
{
    real result=0;

    for(unsigned int i=0; i<3; i++)
        result+=v1[i]*v2[i];

    return result;
}

//Cross product
Vector3D& Vector3D::operator^=(Vector3D const& vector)
{
    real copy[3];
    for(unsigned int i=0; i<3; i++)
        copy[i]=m_components[i];

    m_components[0]=copy[1]*vector[2]-copy[2]*vector[1];
    m_components[1]=copy[2]*vector[0]-copy[0]*vector[2];
    m_components[2]=copy[0]*vector[1]-copy[1]*vector[0];

    return *this;
}
Vector3D operator^(Vector3D const& v1, Vector3D const& v2)
{
    Vector3D copy(v1);
    copy^=v2;
    return copy;
}


void Vector3D::zero()
{
    for (unsigned int i=0; i<3; i++)
        m_components[i]=0.0;
}

real Vector3D::squaredLenght() const
{
    real squaredLenght=0;

    for(unsigned int i=0; i<3; i++)
        squaredLenght+=m_components[i]*m_components[i];

    return squaredLenght;
}

real Vector3D::lenght() const
{
    return sqrt(squaredLenght());
}



void Vector3D::normalize()
{
    real l=lenght();

    if(l>0)
        (*this) *= ((real)1)/l;

}


bool makeOrthonormalBasis(Vector3D &a, Vector3D &b, Vector3D &c)
{
    c=a^b;
    if(c.squaredLenght()==0.0)
        return false;

    a.normalize();
    c.normalize();

    b=c^a;

    return true;
}

}
